/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/offline_scurve_plan.h
 * @Description  : 定义了离线S型规划类，包含了用于路径规划的多种方法和参数设置。
 *                 主要功能包括根据起点和终点位移、速度进行S曲线规划，
 *                 并提供运动参数设置、运动状态获取等接口。
 *                 通过不同的运行模式（正常、挂起、抑制、急停）来管理规划状态，
 *                 确保运动的平滑性和安全性。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0 
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:19:22
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include "plan_target_info.h"
#include <atomic>

constexpr int PROFILE_MODE_NORMAL = 0;
constexpr int PROFILE_MODE_REFRAIN = 1;
constexpr int PROFILE_MODE_SUSPEND = 2;
constexpr int PROFILE_MODE_ESTOP = 3;
// 规划时模式
struct ProfilerMode {
    // int lastRunMode;
    int run_mode; // 正常模式、挂起模式、抑制、急停模式
    // FIXME:需要区分自旋和直线的抑制最大速度
    double vmax;                    // 抑制的最大速度,x
    double dec_q;                   // 缓停减速距离
    std::atomic<bool> ready_replan; // 需要重规划
    ProfilerMode() : run_mode(PROFILE_MODE_NORMAL), vmax(0.2), dec_q(1.5), ready_replan(false) {
        Reset();
    }

    void setMode(int mode)
    {
        // bool ignore = mode == PROFILE_MODE_SUSPEND;
        // if(ignore)
        // {
        //     mode = PROFILE_MODE_NORMAL;
        //     DEBUG_OUT("ProfilerMode:force ignore suspend mode");
        // }
        
        if (mode != run_mode) {
            run_mode = mode;
            ready_replan = true;
            DEBUG_OUT("ProfilerMode: set new mode (0:normal; 1:refrain; 2:suspend; 3:estop) = " << mode);
        }
        //    lastRunMode = runMode;
    }

    int Mode() const {
        return run_mode;
    }

     void Reset() {
        ready_replan = false;
        run_mode = PROFILE_MODE_NORMAL;
        DEBUG_OUT("ProfilerMode: reset to normal mode");
    }
};

// 对称S型规划
class OfflineScurvePlan
{
 public:
    OfflineScurvePlan();
    ~OfflineScurvePlan() = default;

    /// @brief S曲线规划
    /// @param q0 起点位移
    /// @param q1 终点位移
    /// @param v0 起点速度
    /// @param v1 终点速度
    /// @return 按照输入参数和预设加减速参数是否规划成功
    bool Plan(double q0, double q1, double v0, double v1);
    // 周期时间差补运行
    bool Move(int i, double &qi);
    // 周期时间差补运行
    bool Move(int i, double &qi, double &vi);

    /// @brief 以位置模式按规划运行
    /// @param t 从起始经过的时间
    /// @param qi 当前的位置
    /// @param vi 当前的速度
    /// @return 时间是否正确
    bool Move(double t, double &qi, double &vi);

    /// @brief 以速度模式按规划运行
    /// @param i 当前是第几个循环周期
    /// @param vi 速度输出
    /// @return 时间是否正确
    bool MoveVelocity(int i, double &vi); 

    /// @brief 以速度模式按规划运行
    /// @param t 从起始经过的时间
    /// @param vi 速度输出
    /// @return 时间是否正确
    bool MoveVelocity(double t, double &vi);
    
    // 设置系统参数
    void SetSysMotionPara(double vmax, double amax, double jmax);

    // 结束补偿
    void EndCompensation(double surplus_distance);

    // 获取规划的步数
    int Size() { return num_steps_; }

    // 设置周期
    void SetCycle(double cycle);

    // 加速长度
    double GetQa() const { return qa_; }

    // 匀速长度
    double GetQv() const { return qv_; }

    // 减速长度
    double GetQd() const { return qd_; }

    // 减速起始点的距离
    double GetPd() const { return qp4_; }

    // 末段开始时间
    double GetTimeDP3() { return total_time_  - tj2_; }

    // 假设距离足够长，根据系统参数j,a,计算需要的距离
    // bool Plan(double v0, double v1);

    // 获取当前规划速度值
    // relQi: 相对距离 totalQi：当前规划距离
    double PlanVel();

    // 获取当前规划的理论面积
    void GetPlanArea(double& rel_qi, double& total_qi);

    // 已经走过的所有面积和
    double AddNewCoverQ(double add_q);

    // 获取当前运行阶段
    int GetStep() const;

    // 输入当前的实时速度，到减速时，需要多长的距离
    double GetStopDistance(double v) const;

    // 根据两个点参数，计算包含/不包含匀加减阶段的位移
    double DisRequirement(bool chooseMin, double v0, double v1, double amax, double jmax);

    // 重载距离获取函数
    double DisRequirement(bool chooseMin, double v0, double v1);

    // 根据当前位移，查找当前的速度
    double FindV(double cur_q);
    
 public:
    // 规划目标信息
    PlanTargetInfo target_info_;
    // 当前设置的模式
    ProfilerMode plan_mode_;

    // 规划速度的7个阶段，某些阶段可能由于距离，不存在
    enum PlanStep {
        Step1,
        Step2,
        Step3,
        Step4,
        Step5,
        Step6,
        Step7
    };

 private:
    // 重新规划时，复位必要的状态值
    void Reset();

    // 显示关键时间节点
    void ShowKeyTime();

    // bool Plan(double q0, double q1, double v0, double v1, int &N);

    void SetSysMotionPara(double vmin, double vmax, double amin, double amax, double jmin, double jmax);

    // S型规划最低要求
    bool ParaMinDisRequirement();

    // 不符合S规划最低要求时重新计算减速运行参数
    void ReCalPara();

    // void ReCalPara(double& Tj , double& jmax);

    // Tv段不存在时的时间参数计算
    bool TvNotExistTimeParaCal(double gama);

    // Tv段存在时的时间参数计算
    bool TvExistTimeParaCal();

    // 增加对位移减少情况的处理
    void Convert2OppositeCase();

    // 计算实际运行参数
    void CalRealMotionPara();

    // 获取ti的输出位置Qi
    bool GetQi(double ti, double &qi) const;

    // 获取ti的输出速度Vi
    bool GetVi(double ti, double &vi);

    // 获取ti的输出速度Vi
    void GetKeyNode();

    // 初始化系统参数
    void InitSystemPara();

    // 初始化阶段标识
    void InitStepFlag();

    // 初始化关键点参数
    void InitKeyNode();

    // 初始化用户设定参数
    void InitUserPara();

    // 初始化限制参数
    void InitLimitPara();

    // 初始化规划时间参数
    void InitTPara();

    // 获取当前的t对应的位移点
    bool GetPosition(double t, double &qi) const;

    // 获取当前的位移对应的T
    double FindT(double target_q) const;
    
 private:
    int index_;
    double cover_q_;             // 不断消耗的面积(每个路段逐渐累加)
    PlanStep step_;              // 当前运行阶段
    std::atomic<bool> plan_ok_;  // 是否是规划失败

 private:
    // 系统内置参数
    double vmin_, vmax_;
    double amin_, amax_;
    double jmin_, jmax_, jlim_;
    double cycle_;

    // 当前所处的曲线的阶段，区分加速、匀速、减速三段
    bool acc_, uniform_, dec_;

    // 8个关键的速度点
    double vp0_, vp1_, vp2_, vp3_, vp4_, vp5_, vp6_, vp7_;

    // 8个关键的位移点
    double qp0_, qp1_, qp2_, qp3_, qp4_, qp5_, qp6_, qp7_;

    // 加速、匀速、减速的长度
    double qa_, qv_, qd_;

    // 8个关键的加速度点
    double ap0_, ap1_, ap2_, ap3_, ap4_, ap5_, ap6_, ap7_;

    // 用户参数
    double q0_, q1_;
    double v0_, v1_;

    // 根据用户参数调整的系统参数
    double alima_, alimd_;
    double vlima_, vlimd_;
    double vlim_; // real_vlim
    double tcomp_;
    double jcomp_;

    // 规划的时间参数
    double tj1_, tj2_;
    double tv_;
    double ta_, td_;
    double total_time_;

    // 是否为q0>q1
    int sign_;
    int num_steps_;
    int is_error_;
    double previous_t_;
};